#include "UartTask.h"

#define Byte P0
 
xQueueHandle UARTQueue;
extern xQueueHandle GraphQueue;
extern xQueueHandle CommandThreadQueue;
extern xQueueHandle WebQueue;
portTickType xLastWakeTime;
xTimerHandle UARTtimer;								 
static void UARTtimerCallback( xTimerHandle pxTimer );
const portTickType xFrequency = 1000 / portTICK_RATE_MS;
LPC_UART_TypeDef *UARTx;
uint8_t UARTrxbuf[sizeof(struct commMsg)];
uint8_t UARTtxbuf[sizeof(struct commMsg)];
unsigned char* dummy = "Ultrasonic Data";
unsigned char* dummy2 = "Color Data     ";
unsigned char* dummy3 = "Encoder Data   ";
struct commMsg* txMsg;
struct UARTPacket *pxRxedMessage;
struct commMsg txBuf;


void UARTTaskCode( void * pvParameters ){
	 // initialize UART
	 init_UART();

	// Initialize LCD
	GLCD_Init();
	GLCD_Clear(Green);
	GLCD_SetTextColor(Green);
	GLCD_SetBackColor(Blue);
	GLCD_Clear(Blue);

	 //define pointer 
	 uint32_t response = 0;
	 unsigned char buff[4];
	 UARTtimer = xTimerCreate
		          (  /* Just a text name, not used by the RTOS kernel. */
                     (const signed char*)"UARTtimer",
                     /* The timer period in ticks. */
                     ( 1000 / portTICK_RATE_MS  ),
                     /* The timers will auto-reload themselves when they expire. */
                     pdTRUE,
                     /* Assign each timer a unique id equal to its array index. */
                     ( void * ) 3,
                     /* Each timer calls the same callback when it expires. */
                     UARTtimerCallback
                   );
				   	if( xTimerStart( UARTtimer, 0 ) != pdPASS )
             {
                 /* The timer could not be set into the Active state. */				 
             }
	
	 // Go to infinite Loop

	 static Comms_Msg_Struct *Msg;
	 static Comms_Msg_Struct Mymsg;
	 for(;;){
	

		if( UARTQueue != 0 )
    	{
        // Receive a message on the created queue.  Block for 0 ticks if a
        // message is not immediately available.
        	if( xQueueReceive( UARTQueue, (void*)&( Msg ), ( portTickType ) 0 ) )
        		{
					SendMsg(Msg);

				}
		}
	/* print a string */
    //UART_SendByte(UARTx,0x55);
	uint8_t start = UART_ReceiveByte(UARTx);
	if (start==0x55) 
	{
		response = UART_Receive(UARTx, UARTrxbuf, sizeof(struct commMsg), BLOCKING);
		if (response == 9) 
		{
			//Replace with switch statement later
			if (UARTrxbuf[0] == 0x01) 
			{	
				GLCD_DisplayString(0,0,1,dummy);
				print_hex_packet(&UARTrxbuf[4],sizeof(UARTrxbuf[4]),buff);
				GLCD_DisplayString(1,0,1,buff);  
			}
			if (UARTrxbuf[0] == 0x02) 
			{ 
				GLCD_DisplayString(2,0,1,dummy2);
				print_hex_packet(&UARTrxbuf[4],sizeof(UARTrxbuf[4]),buff);
				GLCD_DisplayString(3,0,1,buff);
				print_hex_packet(&UARTrxbuf[6],sizeof(UARTrxbuf[6]),buff);
				GLCD_DisplayString(3,3,1,buff);
				print_hex_packet(&UARTrxbuf[8],sizeof(UARTrxbuf[8]),buff);
				GLCD_DisplayString(3,6,1,buff);
			}
			if (UARTrxbuf[0] == 0x03) 
			{ 
				GLCD_DisplayString(4,0,1,dummy3);
				print_hex_packet(&UARTrxbuf[3],sizeof(UARTrxbuf[3]),buff);
				GLCD_DisplayString(5,0,1,buff);
			}
			

			txBuf.msgType = UARTrxbuf[0];
			txBuf.size = UARTrxbuf[1];
			txBuf.counter = UARTrxbuf[2];
			txBuf.data[0] = UARTrxbuf[3];
			txBuf.data[1] = UARTrxbuf[4];
			txBuf.data[2] = UARTrxbuf[5];
			txBuf.data[3] = UARTrxbuf[6];
			txBuf.data[4] = UARTrxbuf[7];
			txBuf.data[5] = UARTrxbuf[8];

			Mymsg.msgType = UARTrxbuf[0];
			Mymsg.size = UARTrxbuf[1];
			Mymsg.counter = UARTrxbuf[2];
			Mymsg.data[0] = UARTrxbuf[3];
			Mymsg.data[1] = UARTrxbuf[4];
			Mymsg.data[2] = UARTrxbuf[5];
			Mymsg.data[3] = UARTrxbuf[6];
			Mymsg.data[4] = UARTrxbuf[7];
			Mymsg.data[5] = UARTrxbuf[8];
			Msg = &Mymsg;
			
			txMsg = &txBuf;

			if(xQueueSend( WebQueue, (void * ) &txMsg, ( portTickType ) 0 ) != pdPASS){};
			if(xQueueSend( CommandThreadQueue, (void * ) &Msg, ( portTickType ) 0 ) != pdPASS){};

//					if (count == 256) {
//						count = 1;
//					}
//		   UART_SendByte(UARTx, count);
//
//		   count+= 5

				
		}	
		else{
			unsigned char* error = "error";
			GLCD_DisplayString(posY,10,0.5,error);
			posY++;
			if(posY==9){GLCD_Clear(Blue);posY = 6;}
		}		
	 }
	 }
}
 

void Init_UART_Task(){

 static unsigned char ucParameterToPass;
	xTaskHandle UARTHandle;
  xTaskCreate( UARTTaskCode, ( signed char * ) "UART Task", 2*configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY
  , &UARTHandle );
  UARTQueue = xQueueCreate( 100, sizeof(struct UARTPacket)  );

}

static void UARTtimerCallback( xTimerHandle pxTimer )
{	    
		static Comms_Msg_Struct *Mymsg;
		static Comms_Msg_Struct MotorControllMsg;
		MotorControllMsg.msgType = 0x01;
       	MotorControllMsg.counter = 0;
       	MotorControllMsg.size = 2;
       	MotorControllMsg.data[0] = 10;
       	MotorControllMsg.data[1] = 0;
       	MotorControllMsg.data[2] = 3;
       	MotorControllMsg.data[3] = 4;
       	MotorControllMsg.data[4]  = 5;
       	MotorControllMsg.data[5]  = 6;
		Mymsg = &MotorControllMsg;
	  if( CommandThreadQueue != 0 )
	  { 				 
      //	if( xQueueSend( CommandThreadQueue, ( void * ) &Mymsg, ( portTickType ) 10 ) != pdPASS )
       //	{
           						 // Failed to post the message
	   // }	
			
	  }
}


void init_UART(){

        UARTx = (LPC_UART_TypeDef *)LPC_UART1;

        /* UART Configuration structure variable */
        UART_CFG_Type UARTConfigStruct;
        /* UART FIFO configuration Struct variable */
        UART_FIFO_CFG_Type UARTFIFOConfigStruct;
        /* Pin configuration for UART0 */
        PINSEL_CFG_Type PinCfg;

        if((uint32_t)UARTx == (uint32_t)LPC_UART0) {
                /*
                 * Initialize UART0 pin connect
                 */
                PinCfg.Funcnum = 1;
                PinCfg.OpenDrain = 0;
                PinCfg.Pinmode = 0;
                PinCfg.Pinnum = 2;
                PinCfg.Portnum = 0;
                PINSEL_ConfigPin(&PinCfg);
                PinCfg.Pinnum = 3;
                PINSEL_ConfigPin(&PinCfg);
        }
        else if ((uint32_t)UARTx == (uint32_t)LPC_UART1) {
                /*
                 * Initialize UART1 pin connect
                 */
                PinCfg.Funcnum = 2;
                PinCfg.OpenDrain = 0;
                PinCfg.Pinmode = 0;
                PinCfg.Pinnum = 0;
                PinCfg.Portnum = 2;
                PINSEL_ConfigPin(&PinCfg);
                PinCfg.Pinnum = 1;
                PINSEL_ConfigPin(&PinCfg);
        }

        /* Initialize UART Configuration parameter structure to default state:
         * Baudrate = 9600bps
         * 8 data bit
         * 1 Stop bit
         * None parity
         */
        UART_ConfigStructInit(&UARTConfigStruct);
		//UARTConfigStruct.Parity = UART_PARITY_SP_1;
		//UARTConfigStruct.Stopbits = UART_STOPBIT_1;
		//UARTConfigStruct.Databits = UART_DATABIT_8;

        /* Set Baudrate to 19200 */
        UARTConfigStruct.Baud_rate = 9600;

        /* Initialize UART0 peripheral with given to corresponding parameter */
        UART_Init(UARTx, &UARTConfigStruct);

        /* Initialize FIFOConfigStruct to default state:
         *                              - FIFO_DMAMode = DISABLE
         *                              - FIFO_Level = UART_FIFO_TRGLEV0
         *                              - FIFO_ResetRxBuf = ENABLE
         *                              - FIFO_ResetTxBuf = ENABLE
         *                              - FIFO_State = ENABLE
         */
        UART_FIFOConfigStructInit(&UARTFIFOConfigStruct);
		
        /* Initialize FIFO for UART0 peripheral */
        UART_FIFOConfig(UARTx, &UARTFIFOConfigStruct);

        /*  Enable UART Transmit */
        UART_TxCmd(UARTx, ENABLE);

		/* Enable UART Rx interrupt */
    UART_IntConfig(UARTx, UART_INTCFG_RBR, ENABLE);
    /* Enable UART line status interrupt */
    //UART_IntConfig(UARTx, UART_INTCFG_RLS, ENABLE);
    /*
     * Do not enable transmit interrupt here, since it is handled by
     * UART_Send() function, just to reset Tx Interrupt state for the
     * first time
     */
    TxIntStat = RESET;
 
    // Reset ring buf head and tail idx
    __BUF_RESET(rb.rx_head);
    __BUF_RESET(rb.rx_tail);
    __BUF_RESET(rb.tx_head);
    __BUF_RESET(rb.tx_tail);
 
    /* preemption = 1, sub-priority = 1 */
    NVIC_SetPriority(UART0_IRQn, ((0x01<<3)|0x01));
    /* Enable Interrupt for UART0 channel */
    NVIC_EnableIRQ(UART0_IRQn);
}

void print_hex_packet(uint8_t *packet,uint16_t size,unsigned char *Buffer){
        const char *hex_Map = "0123456789ABCDEF";
        //char Buffer[4];

        int i,j;
        j=0;
        for (i=0;i<size;i++){
                        Buffer[j++] = hex_Map[ packet[i] >>    4 ];
                        Buffer[j++] = hex_Map[ packet[i] &  0x0f ];
                        if (i<5) Buffer[j++] = ',';
                }
                Buffer[j++] = 0;
                //printf("  ");
                //printf (Buffer, sizeof (Buffer));
}

void send_start() {

	UARTtxbuf[0] = 0x04;
	UARTtxbuf[1] = 2;
	UARTtxbuf[2] = 0x0;
	UARTtxbuf[3] = 127;
	UARTtxbuf[4] = 255;
	UARTtxbuf[5] = 0x5;
	UARTtxbuf[6] = 0x6;
	UARTtxbuf[7] = 0x7;
	UARTtxbuf[8] = 0x8;
	UART_Send(UARTx, UARTtxbuf, 9, BLOCKING);

}

void send_stop() {
 	
	UARTtxbuf[0] = 0x04;
	UARTtxbuf[1] = 2;
	UARTtxbuf[2] = 0x0;
	UARTtxbuf[3] = 0x40;
	UARTtxbuf[4] = 0xC0;
	UARTtxbuf[5] = 0x5;
	UARTtxbuf[6] = 0x6;
	UARTtxbuf[7] = 0x7;
	UARTtxbuf[8] = 0x8;
	UART_Send(UARTx, UARTtxbuf, 9, BLOCKING);

}

void SendMsg(Comms_Msg_Struct* Msg) {
 	
	UARTtxbuf[0] = Msg->msgType;
	UARTtxbuf[1] = Msg->counter;
	UARTtxbuf[2] = Msg->size;
	UARTtxbuf[3] = Msg->data[0];
	UARTtxbuf[4] = Msg->data[1];
	UARTtxbuf[5] = Msg->data[2];
	UARTtxbuf[6] = Msg->data[3];
	UARTtxbuf[7] = Msg->data[4];
	UARTtxbuf[8] = Msg->data[5];
	UART_Send(UARTx, UARTtxbuf, 9, BLOCKING);

}